%author: wxj233
%time: 2023.11.10 10:00
%function:  轨迹


classdef Track < handle
    %TRACK 此处显示有关此类的摘要
    %   此处显示详细说明
    
    properties
        id  % 轨迹id
        fps  % feature points特征点[fp1, fp2, ...]单个fp包含{points, Xk_e, Pk_ee, fplife}
        tlifeMax  % 轨迹最大生命值

        validFpNum  % 本次跟新有效量测数量
        tlife  % track life,轨迹当前生命值，每记忆一次减一
        mNum  % 量测数量(measure Number),有效量测次数,每有效关联一次加一,轨迹初始时为0，也就是初始的点不计算进来，也不保存
        len  % 轨迹总长度,每迭代一次加一，轨迹初始时为0，也就是初始的点不计算进来，也不保存
        gXk_e  % 群状态[x1, vx1, y, vy1, x2, vx2, y2, vy2, ...]'
        gPk_ee  % 群状态估计误差协方差矩阵
        mPcov  % 中心点状态，协方差矩阵

        points  % 群状态记录[t, x, vx, y, vy; ...]，位置取均值
        mfpTracer  % 该轨迹所属追踪器
    end
    
    methods
        function obj = Track(tlifeMax, mfpTracer)
            %TRACK 构造此类的实例
            % tlifeMax: 轨迹最大生命值
            obj.id = obj.getId();  % 自动递增id
            obj.fps = struct([]);
            obj.tlifeMax = tlifeMax;
            obj.mNum = 0;
            obj.len = 0;
            obj.tlife = obj.tlifeMax;
            obj.validFpNum = 0;
            obj.points = [];
            obj.mfpTracer = mfpTracer;
            obj.gXk_e = [];
            obj.gPk_ee = [];
        end
        

        function vfusion(obj, p)
            % 速度融合
            % p: 群相关性因子，取0~1，0代表不相关，1代表强相关
            % 完成后更新群状态，群协方差矩阵，更新特征点的状态以及协方差矩阵
            fpNum = length(obj.fps);
            F = zeros([fpNum*4, fpNum*4]);  % 融合矩阵
            if obj.validFpNum > 0  % 有有效量测关联时
                for i = 1:1:fpNum
                    if obj.fps(i).isValidAss
                        F((i-1)*4+1, (i-1)*4+1) = 1;  % x
                        F(2:4:fpNum*4, (i-1)*4+2) = p/obj.validFpNum;
                        F((i-1)*4+2, (i-1)*4+2) = 1-p+p/obj.validFpNum; % vx
    
                        F((i-1)*4+3, (i-1)*4+3) = 1;  % y
                        F(4:4:fpNum*4, (i-1)*4+4) = p/obj.validFpNum;
                        F((i-1)*4+4, (i-1)*4+4) = 1-p+p/obj.validFpNum; % vy
                    else
                        F((i-1)*4+1, (i-1)*4+1) = 1;  % x
                        F((i-1)*4+2, (i-1)*4+2) = 1-p; % vx
    
                        F((i-1)*4+3, (i-1)*4+3) = 1;  % y
                        F((i-1)*4+4, (i-1)*4+4) = 1-p; % vy
                    end
                end
            else  % 没有有效量测关联
                for i = 1:1:fpNum
                    F((i-1)*4+1, (i-1)*4+1) = 1;  % x
                    F(2:4:fpNum*4, (i-1)*4+2) = p/fpNum;  % 既然没有有效量测，那就每一个特征点都贡献一份力量
                    F((i-1)*4+2, (i-1)*4+2) = 1-p + p/fpNum; % vx

                    F((i-1)*4+3, (i-1)*4+3) = 1;  % y
                    F(4:4:fpNum*4, (i-1)*4+4) = p/fpNum;
                    F((i-1)*4+4, (i-1)*4+4) = 1-p+p/fpNum; % vy
                end
            end

%             if F ~= eye(4)
%                 disp("bug");
%             end
            obj.gXk_e = F*obj.gXk_e;

            obj.gPk_ee = F*obj.gPk_ee*F';
            obj.gPk_ee = (obj.gPk_ee+obj.gPk_ee')/2;

            for i = 1:1:fpNum
                obj.fps(i).Xk_e = obj.gXk_e((i-1)*4+1:(i-1)*4+4);
                obj.fps(i).Pk_ee = obj.gPk_ee((i-1)*4+1:(i-1)*4+4, (i-1)*4+1:(i-1)*4+4);
            end
        end


        function predict(obj, dt, q)
            % 轨迹预测完成每个特征点的预测
            % dt: 预测时间间隔
            % q: 过程噪声量级
            fpNum = length(obj.fps);
            gA = [];
            gQ = [];
            for i = 1:1:fpNum
                [A, Q] = obj.fps(i).predict(dt, q);  % 同时更新了特征点内部状态
                gA = obj.diagcat(gA, A);
                gQ = obj.diagcat(gQ, Q);
                obj.gXk_e((i-1)*4+1:(i-1)*4+4) = obj.fps(i).Xk_e;  % 群状态更新
            end

            obj.gPk_ee = gA*obj.gPk_ee*gA' + gQ;  % 更新群状态预测误差协方差矩阵,这个地方由于精度问题出现了不对称情况
            obj.gPk_ee = (obj.gPk_ee+obj.gPk_ee')/2;

        end


        function update(obj, dr, dtheta, dvr, nowTime)
            % 轨迹更新完成每个特征点的更新
            % dr: 距离量测方差
            % dtheta: 角度量测方差
            % dvr: 速度量测方差
            % nowTime: 要更新的时刻
            obj.len = obj.len + 1;
            if obj.validFpNum > 0
                obj.mNum = obj.mNum + 1;
                obj.tlife = obj.tlifeMax;
            else
                obj.tlife = obj.tlife - 1;
                if obj.tlife == 0
                    obj.mfpTracer.deleteTrack(obj.id);
                    return;
                end
            end

            [gZk_p, gPz, gPxz] = UT(@obj.gH, obj.gXk_e, obj.gPk_ee, 3*length(obj.fps));  % 量测预测、方差、以及和原状态的协方差,通过容积变换计算

%             gSk = gPz+gR;
%             Gk = gPxz/gSk;
%             obj.gPk_ee = obj.gPk_ee-Gk*gSk*Gk';
%             obj.gPk_ee = (obj.gPk_ee+obj.gPk_ee')/2;

            gKk = [];
            gR = [];
            state = zeros([1, 5]);
            if obj.validFpNum > 0  % 根据匹配数量进行量测方差缩放
               dr = dr/obj.validFpNum;
               dtheta = dtheta/obj.validFpNum;
               dvr = dvr/obj.validFpNum;
            end

            i = 0;
            for fp = obj.fps
                Zk_p = gZk_p(i*3+1:i*3+3, :);
                Pz = gPz(i*3+1:i*3+3, i*3+1:i*3+3);
                Pxz = gPxz(i*4+1:i*4+4, i*3+1:i*3+3);
                [Kk, R, isExist] = fp.update_new(fp.assP, dr, dtheta, dvr, nowTime, Zk_p, Pz, Pxz);
                if isExist
                    gKk = obj.diagcat(gKk, Kk);
                    gR = obj.diagcat(gR, R);
                    obj.gXk_e(4*i+1:4*i+4, :) = fp.Xk_e;
                    i = i + 1;
                    state = state + fp.points(end, [1, 13:16]);
                else  % 不存在了就删除相应状态
                    gZk_p(i*3+1:i*3+3, :) = [];
                    gPz(i*3+1:i*3+3, :) = [];
                    gPz(:, i*3+1:i*3+3) = [];
                    gPxz(i*4+1:i*4+4, :) = [];
                    gPxz(:, i*3+1:i*3+3) = [];
                end
            end



            
            obj.gPk_ee = obj.gPk_ee - 2*gPxz*gKk' + gKk*(gR + gPz)*gKk';
            obj.gPk_ee = (obj.gPk_ee+obj.gPk_ee')/2;
%             disp(obj.gPk_ee);

            obj.points = [obj.points; state/length(obj.fps)]; % 新增point

            F = [];
            for i = 1:1:length(obj.fps)
                obj.fps(i).Pk_ee = obj.gPk_ee(4*i-3:4*i, 4*i-3:4*i);
                F = [F, eye(4)/length(obj.fps)];
            end
            obj.mPcov = F*obj.gPk_ee*F';  % 均值点的协方差矩阵
        end


        function fp = addFp(obj, p1, p2, dr, dtheta)
            % 添加特征点，同时更新群状态,起始时fplifeMax自行设定
            % p1: t1时刻特征点[1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster; ...]
            % p2: t2时刻特征点[1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster; ...]
            % dr: 测量距离方差
            % dtheta: 测量角度方差
            % return fp: fp中新的点为[1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster, 13Xe, 14Vxe, 15Ye, 16Vye; ...]
            if p1(1) == obj.mfpTracer.times(1)  % 起始时fplifeMax自行设定
                fplifeMax = 1;
            else
                fplifeMax = 2;
            end
            
            dt = p2(1) - p1(1);
            Xe = [p1([2, 3]), p2([2, 3])]';
            Pe = [dr, 0, 0, 0;
                0, dtheta, 0, 0;
                0, 0, dr, 0;
                0, 0, 0, dtheta];
            [Xk_e, Pk_ee, ~] = UT(@obj.calXk_e, Xe, Pe, 4, dt);  % 这个地方应该有更精确的状态求解，但是我目前赶时间径向速度都没用

            ps = [p1, p1(10), Xk_e(2,1), p1(11), Xk_e(4,1);
                  p2, Xk_e'];
            fp = FeaturePoint(fplifeMax, ps, Xk_e, Pk_ee, obj);
            obj.fps = [obj.fps, fp];

            obj.gXk_e = [obj.gXk_e; Xk_e];

            obj.gPk_ee = obj.diagcat(obj.gPk_ee, Pk_ee);

            obj.validFpNum = obj.validFpNum + 1;
        end


        function M = diagcat(~, M, m)
            % 完成子矩阵的对角线拼接
            % M: 被拼接的矩阵
            % m: 将被拼接进M的矩阵m
            lb = zeros(size(m,1), size(M, 2));
            ru = zeros(size(M,1), size(m, 2));
            M = [M,ru;lb,m];
        end


        function Xk_e = calXk_e(~, Xe, dt)
            % Xe: [r1, theta1, r2, theta2]'
            % dt: 时间差
            % return V: 速度
            L1 = [Xe(1,1)*cos(Xe(2,1)), Xe(1,1)*sin(Xe(2,1))];
            L2 = [Xe(3,1)*cos(Xe(4,1)), Xe(3,1)*sin(Xe(4,1))];
            V = (L2 - L1)/dt;
            Xk_e = [L2(1), V(1), L2(2), V(2)]';
        end


        function Zk = gH(obj, gXk_e)
          % 非线性变换函数
          % Xk_e: 原状态[x, vx, y, vy]'
          Zk = [];
          for i = 1:1:length(obj.fps)
              Xk_e = gXk_e(4*i-3:4*i, :);
              Zk= [Zk; obj.h(Xk_e)];
          end
        end


        function Zk = h(~, Xk_e)
          % 非线性变换函数
          % Xk_e: 原状态[x, vx, y, vy]'
          x = Xk_e(1,1);
          vx = Xk_e(2,1);
          y = Xk_e(3,1);
          vy = Xk_e(4,1);
        
          r = sqrt(x*x+y*y);
          theta = atan2(y, x);
          vr = [x, y]*[vx, vy]'/norm([x, y]);
        
          Zk = [r, theta, vr]';
        end


        function deleteFp(obj, id)
            % 删除指定id的特征点
            for i = 1:1:length(obj.fps)
                if obj.fps(i).id == id
                    obj.mfpTracer.deadFps = [obj.mfpTracer.deadFps, obj.fps(i)];
                    obj.fps(i) = [];
                    obj.gXk_e((i-1)*4+1:1:(i-1)*4+4, :) = [];
                    obj.gPk_ee((i-1)*4+1:1:(i-1)*4+4, :) = [];
                    obj.gPk_ee(:, (i-1)*4+1:1:(i-1)*4+4) = [];
                    break;
                end
            end

            if isempty(obj.fps)
                obj.mfpTracer.deleteTrack(obj.id);
            end
        end

    end


    methods (Static)
        function id = getId(varargin)
          % 获取特征点唯一标识
          persistent ID;
          if size(varargin, 2) == 1
                ID = varargin{1};
                return;
          end

          if isempty(ID)
             ID = 0;
          end
          ID = ID + 1;
          id = ID;
        end
    end
end

